package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.image.NIVisionException;

public class FRC2012 extends IterativeRobot
{
    private boolean[] ballPos;
    //The different objects.
    private Input input;
    private Output output;
    private FBW robotThink;
    private Debug debug;
    private Vision vision;
    private EnhancedIO systemIO;

    private double launcherPower;
    private boolean fireRequested = false;
    private boolean enb;
    private boolean keepTopOn = true;
    private boolean keepTopOnII = true;

    //The Y values of both joysticks.
    double leftStickY;
    double rightStickY;

    //Range sensor values
    private int curRange;
    private String strRange;

    //Target coorginates
    double targetX;
    double targetY;

    //Ball managment variables
    private double rollerPower;
    private double towerPower;
    private double pusherPower;
    private double shooterPower;

    private int autonStep;
    private Timer autonTimer;

    private boolean lightState = false;
    private boolean shooterMotorState = false;
    private boolean toggleJ3B11State = false;

    private boolean[] ballSystemPosVals;
    private double[] ballSystemMotorVals;

    public void robotInit() {
        //Initialize the different objects.
        this.input = new Input();
        this.output = new Output();
        this.robotThink = new FBW(); //el
        this.debug = new Debug();
        //this.vision = new Vision();
        this.launcherPower = 0;
        this.leftStickY = 0;
        this.rightStickY = 0;
        this.autonStep = 0;
        this.enb = false;
        this.ballPos = new boolean[3];
    }

    public void disabledInit() {

    }

    public void autonomousInit() {
        this.autonTimer = new Timer();
        this.autonTimer.start();
    }

    public void teleopInit() {

    }

    public void disabledPeriodic() {

        //output.setRelay(1, true);
        //output.setRelay(2, true);
        //output.setRelay(3, true);
        //output.setRelay(4, true);

        int rangeSensor;
        rangeSensor = this.input.getRange();
        double rangeAnswer;
        rangeAnswer = this.robotThink.calcRange(rangeSensor);
        
        //vision.trackTargetX();
        /*try {
            vision.getConvex().write("blaw.png");
        } catch (NIVisionException ex) {
            ex.printStackTrace();
            System.out.println("NI Exception");
        } catch (NullPointerException ex) {
            ex.printStackTrace();
            System.out.println("Null Pointer");
        }*/
        
        
        debug.clearScreen();
        debug.writeToScreen("Raw :" + new Double(rangeSensor).toString(), 0);
        debug.writeToScreen("Inches: " + new Double(rangeAnswer).toString(), 1);
        //debug.writeToScreen(new  Double(input.leftArm.getY()).toString());
        //debug.writeToScreen(new  Double(input.rightArm.getY()).toString());
    }

  public void autonomousPeriodic() {
        /**
         * Autonomous plan:
         * 1) wait for kinect OK
         * 2) acquire target (1-2 seconds): Use vision process to assess horizontal offset.
         * 3) adjust by spinning left or right,low motor values to not overcompensate.
         * 4) adjust angle of the shooter according to feedback from range sensor 
         * 5) Fire the two pre-loaded shots
         * 6) turn around
         * 7) move to center ramp, bring it down.
         * 8) absorb balls
         * ------ CAN END HERE DEPENDING ON TIME ----
         * 9) acquire target
         * 10) adjust
         * 11) fire
         * 12) get out of way
         */
        double rightMotor = 0;
        double leftMotor = 0;

        switch(this.autonStep) {
            case 0:
                if(this.autonTimer.get() > 2000000){
                    this.autonStep++;
                }
                break;
            case 1:
                this.autonStep++;
                break;
            case 2:
                if(input.getKinectState() == 0) {
                    this.autonStep++;
                }
                break;
            case 3:
                if(targetX < -0.1) {
                    rightMotor = 0.5;
                    leftMotor = -0.5;
                } else if(targetX > 0.1) {
                    rightMotor = -0.5;
                    leftMotor = 0.5;
                } else if(targetX > -0.1 && targetX < 0.1) {
                    rightMotor = 0;
                    leftMotor = 0;
                    this.autonStep++;
                }
                break;
            case 4:
                //Shoot
            //    output.setLauncher(0.9);
                //Commented out because a less complicated method has been written.
                /*boolean[] getBallPos;
                getBallPos = input.getBallPosition();
                if(getBallPos[0]&& getBallPos[1] && getBallPos[2]){
                *    
                }*/

                break;
            case 5:
                //Turn around
                break;
            case 6:
                //Move to center ramp and bring ramp down
                break;
            case 7:
                //Eat balls 
                break;
            case 8:
                break;
            case 9:
                break;
                default:
                System.out.println("Unknown autonStep");
                break;
                
        }
        
        double[] autonMotorVals = robotThink.processVals(leftMotor, rightMotor);
        output.setMotors(autonMotorVals[0], autonMotorVals[1]);
        
        //Commented out becasue a less complex method has been written.
        /*ballSystemPosVals = input.getBallPosition();
        ballSystemMotorVals = robotThink.calcBallSystemVals(ballSystemPosVals);
        output.setBallSystem(ballSystemMotorVals);
        * 
        */

    }
    
  // MOVE THESE TO robotInit or teleopInit
    boolean bManualMode = false;
    boolean shooterMotor = false;
    double coDriverThrottle = 0.0;
    double launcherSpeed = 0.0;           
    double shooterAngle = 45.0; 
    //
    public void teleopPeriodic() {
 
/*
         
        if(input.getStickButtonState(input.rightStick, 1) == true) {
            debug.writeToScreen("PULL",5);
            
            try {
                vision.getConvex().write("blaws.png");
            } catch (NIVisionException ex) {
                ex.printStackTrace();
                System.out.println("NI Exception");
            } catch (NullPointerException ex) {
                ex.printStackTrace();
                System.out.println("Null Pointer");
            }
        } else {
            debug.writeToScreen("       ",5);
        }
 */       
/*
        //vision.trackTargetX();
        // Set launcherPower to z-Axis
        this.launcherPower = this.input.getAxis(this.input.rightStick, this.input.zAxis);
        this.output.setLauncher(this.launcherPower);
*/
        
     /*   if(input.getStickButtonState(input.rightStick, 11) == true && enb == false) {
            output.setLight(true);
            enb = true;
        } else if(enb == true){
            output.setLight(false);
        }
        * 
        */

        /*
        //Update the LCD every time.
        DriverStationLCD.getInstance().updateLCD();
        debug.writeToScreen(debug.intToString(input.getRange()), 0);
        */

        //Set the variables to their values.
        leftStickY = this.input.getAxis(this.input.leftStick, this.input.yAxis);
        debug.writeToScreen("LeftStickY - " + leftStickY,3);
        rightStickY = this.input.getAxis(this.input.rightStick, this.input.yAxis);
        debug.writeToScreen("RightStickY - " + rightStickY,4);
        
        
        //if(input.getStickButtonState(input.leftStick, 1)){
        //    leftStickY *= 0.5;
        //    rightStickY *= 0.5;           
        //}
         
        //Button 4 Spin Left
        if(input.getStickButtonState(input.leftStick,4) || input.getStickButtonState(input.rightStick,4))
        {
            leftStickY = -0.5;
            rightStickY = 0.5;

        }
        //Button 5 Spin Right
        if(input.getStickButtonState(input.leftStick,5) || input.getStickButtonState(input.rightStick,5))
        {
            leftStickY = 0.5;
            rightStickY = -0.5;
        }

        //Button 3 Half Speed
        if (input.getStickButtonState(input.leftStick, 3) || input.getStickButtonState(input.rightStick,3))
        {
            leftStickY*=0.5;
            rightStickY*=0.5;
        }
        
        /*if (input.getStickButtonState(input.rightStick,3)) {
            output.setLight(true);
        } else {
            output.setLight(false);
        }
        
        * 
        */
        
        /*double coDriThrot = input.getAxis(input.coDriverStick,input.zAxis);
        System.out.println("Throttle raw val: " + coDriThrot);
        * 
        */

        //Move the motors
        //this.output.setMotors(leftStickY, rightStickY); //For raw movement
        double[] motorvals = robotThink.processVals(leftStickY, rightStickY);
        output.setMotors(motorvals[0], motorvals[1]);

        //Compute IO states from Kinect positions
        /*
        try {
            switch(input.getKinectState()) {
            case 0:
                break;
            case 1:
                //Both arms are up
                systemIO.setIOState(0,true);
                systemIO.setIOState(1,true);
                break;
            case 2:
                //Both arms are down
                systemIO.setIOState(0,false);
                systemIO.setIOState(1,false);
                break;
            case 3:
                //Left arm up, right arm down
                systemIO.setIOState(0,true);
                systemIO.setIOState(1,false);
                break;
            case 4:
                //Left arm down, right arm up
                systemIO.setIOState(0,false);
                systemIO.setIOState(1,true);
                break;
            default:
                //Other (Error)
                systemIO.setIOState(0,false);
                systemIO.setIOState(1,false);
                System.out.println("Invalid Kinect state");
                break;
            }
        }
        catch(Exception e) {
        }
        */





       // BUTTON 11: ENABLE LIGHT (TOGGLE)
     
        if(input.getStickButtonState(input.leftStick,11)|| input.getStickButtonState(input.rightStick,11))
        {
            output.setLight(true);
            //output.setLight(lightState);
            // TODO what is is?
            //toggleJ3B11State = true;
            //lightState = !lightState;
        }
        if(input.getStickButtonState(input.leftStick,10) || input.getStickButtonState(input.rightStick,10))
        {
            output.setLight(false);
            //if (toggleJ3B11State) lightState = !lightState;
            //output.setLight(lightState);
            //toggleJ3B11State = false;
        }
       //Throttle Shoot Motor Speed
     //Read value from throttle --> spin shooter motor at value between 0.5 and 0.9
       
       
       // coDriverThrottle = input.getThrottle(input.coDriverStick)/255.0;
       // launcherSpeed = 0.5 + coDriverThrottle*0.4;
       
        //Button 2 Shoot Motor, On
        if(input.getStickButtonState(input.coDriverStick,2))
        {  
            shooterMotor = true;
        }
        //Button 3 Shoot Motor, Off
        if(input.getStickButtonState(input.coDriverStick,3))
        {   
            shooterMotor = false;
        }   
            
        if(shooterMotor)
        {
            coDriverThrottle = (input.getAxis(input.coDriverStick,input.zAxis)+1)/2;
         //changed to negative, does it work?
            launcherSpeed = (0.3 + coDriverThrottle*0.5);
            output.setLauncher(launcherSpeed);
        }
        else 
        {
            launcherSpeed = 0.0;
            output.setLauncher(0.0);
        }
        
        /*if (shooterMotor)
        {
            output.setLight(true);
        }
        else
        {
            output.setLight(false);
        }
        * 
        */
        debug.writeToScreen("Shooter Speed :" + launcherSpeed);
        //debug.writeToScreen("Throttle: " + input.getThrottle(input.coDriverStick) );
        
        boolean bBallSensorHIGH = !input.getInput(3);
        boolean bBallSensorMID = !input.getInput(2);
        boolean bBallSensorLOW = !input.getInput(1);
        boolean bTopMotorON = output.getTop() > 0.0;
        boolean bFeederMotorON = output.getFeeder()> 0.0;
        
        //Automagic ball management.
        //boolean bFeedBallUp = !bManualMode && ((!fireRequested && !bTopMotorON && !bBallSensorHIGH && bBallSensorMID)  || (bBallSensorLOW && !bBallSensorMID) || (!bBallSensorHIGH && bBallSensorMID)) ;
        boolean bFeedBallUp = (!bManualMode && ((!bBallSensorHIGH) || (!bBallSensorMID))) || input.getStickButtonState(input.coDriverStick, 1);
        //boolean bFeedBallIn = (!bManualMode && (bFeederMotorON && !bBallSensorLOW));
        boolean bFeedBallIn = (!bManualMode && (bFeedBallUp || !bBallSensorLOW));
        //These booleans are for manual motor control.
        boolean bJ3Button06 = input.getStickButtonState(input.coDriverStick,6);
        boolean bJ3Button07 = input.getStickButtonState(input.coDriverStick,7);
        boolean bJ3Button08 = input.getStickButtonState(input.coDriverStick,8);
        boolean bJ3Button09 = input.getStickButtonState(input.coDriverStick, 9);
        boolean bJ3Button10 = input.getStickButtonState(input.coDriverStick,10);
        boolean bJ3Button11 = input.getStickButtonState(input.coDriverStick,11);
      /*  //These are the booleans for the manual motor control buttons.
        boolean bTopMotorFWD = bManualMode && bJ3Button06;
        boolean bTopMotorREV = bManualMode && bJ3Button07;
        boolean bMiddleMotorFWD = bManualMode && bJ3Button08;
        boolean bMiddleMotorREV = bManualMode && bJ3Button09;
        boolean bBottomMotorFWD = bManualMode && bJ3Button10;
        boolean bBottomMotorREV = bManualMode && bJ3Button11;
        
        * 
        */
        //fireRequested= false;
        
        /*boolean bTopMotorMoveON =   (!bManualMode && ((fireRequested && (bBallSensorHIGH))
                                    || (!fireRequested && !bBallSensorHIGH && bBallSensorMID)));
        
        boolean bTopMotorMoveOFF = !bManualMode && ((fireRequested && !bBallSensorHIGH) || (!fireRequested && bBallSensorHIGH));
        */
        boolean bTopMotorMoveON = (!bManualMode && !bBallSensorHIGH) || input.getStickButtonState(input.coDriverStick, 1);
        boolean bTopMotorMoveOFF = false;
        
        /*if(shooterMotor && input.getStickButtonState(input.coDriverStick,1))
        {   
            fireRequested = true;
            if(keepTopOnII) {
                output.setTop(-0.9);
                keepTopOnII = false;
                System.out.println("MORE POWER!");
            } 
            bTopMotorMoveOFF = false;
            bTopMotorMoveON = false;
            //shooterPower = 0.9;
            //output.setTop(0.9);
        } else {
            keepTopOnII = true;
        }*/
        
        if (bFeedBallUp)// || bMiddleMotorFWD)
        {
            output.setMiddle(0.9);
        } else {
            output.setMiddle(0);
        }
        
        //Button 1 Shoot Ball, with method to bring up balls if none are available.
        if(bTopMotorMoveON) //|| bTopMotorFWD)
        {
          output.setTop(-0.9);
          /*  if(keepTopOn) {
            output.setTop(-0.9);
            keepTopOn = false;
          }*/
        } else {
            //keepTopOn = true;
            output.setTop(0);
        }
        
       /* if (bTopMotorREV)
        {
          output.setTop(-0.9);  
        }
        
        * 
        */
        if (bTopMotorMoveOFF)// && !bTopMotorFWD && !bTopMotorREV)
        {
           fireRequested = false;
           output.setTop(0.0);
        }
         
        
        
       /* if(bMiddleMotorREV)
        {
           output.setMiddle(-0.9); 
        }
             
             * 
             */
        /*else
        {
            output.setMiddle(0.0);
        }*/
        
        
        if (bFeedBallIn)// || bBottomMotorFWD)
        {
            output.setBottom(0.9);
        }
        else
        {
            output.setBottom(0.0);
        }

                /* //Commented out because a less complex method has been written.
        ballSystemPosVals = input.getBallPosition();
        ballSystemMotorVals = robotThink.calcBallSystemVals(ballSystemPosVals);
        output.setBallSystem(ballSystemMotorVals);
       */

        boolean bBallConsume = input.getStickButtonState(input.leftStick,1) || input.getStickButtonState(input.rightStick,1);
        boolean bBallEject = input.getStickButtonState(input.leftStick,2)  || input.getStickButtonState(input.rightStick,2);
        //Button 1 Eat Ball: Turns on only the bottom two ball managment motors.
        if(bBallConsume)
        {
            output.setFeeder(0.9);
        }       
        //Button 2 Ball Eject: Reverses all ball managment motors.       
        if(bBallEject)
        {
            output.setFeeder(-0.9);
        }        
        if (!bBallConsume && !bBallEject)
        {
            output.setFeeder(0.0);
        }
       
        
        //Angle methods NOT YET COMPLETED
          /*  if(input.getStickButtonState(input.coDriverStick,6) == true)
            {
            //Button 6 45deg, sensors, motors
                shooterAngle = 45;
            }
            if(input.getStickButtonState(input.coDriverStick,7) == true)
            {
            //Button 7 30deg, sensors, motors
                shooterAngle = 30;
            }
            if(input.getStickButtonState(input.coDriverStick,8) == true)
            {    
            //Button 8 15deg, sensors, motors
                shooterAngle =15;
            }
            * 
            */ 
            //These booleans are used to change the angle up and down.
            boolean bButtonAngleUP = input.getStickButtonState(input.coDriverStick, 4);
            boolean bButtonAngleDOWN = input.getStickButtonState(input.coDriverStick,5);
            //These booleans are for manual motor control.
            boolean bJ12Button08 = input.getStickButtonState(input.leftStick,8) || input.getStickButtonState(input.rightStick,8);
            boolean bJ12Button09 = input.getStickButtonState(input.leftStick, 9) || input.getStickButtonState(input.rightStick, 9);
            //These booleans are for the ramp rod.
            boolean bJ12Button06 = input.getStickButtonState(input.leftStick, 6) || input.getStickButtonState(input.rightStick, 6);
            boolean bJ12Button07 = input.getStickButtonState(input.leftStick, 7) || input.getStickButtonState(input.rightStick, 7);
            //Shooter angle.
            if(bButtonAngleUP)
            {
                output.setAngle(0.6);
            }
            if(bButtonAngleDOWN)
            {
                output.setAngle(-0.6);
            }
            if(!bButtonAngleUP && !bButtonAngleDOWN)
            {
                output.setAngle(0.0);
            }
            if(bBallSensorLOW && !bBallSensorMID)
            {
                output.setBottom(0.9);
            }
           //Manual mode.
            if (bJ12Button08)
            {
                bManualMode = true;
            }
            if (bJ12Button09)
            {
                bManualMode = false;
            }
            //Ramp rod.
            if (bJ12Button06)
            {
                output.setRelayForward(2,true);
                output.setRelayReverse(2,false);
            }
            if (bJ12Button07)
            {
                output.setRelayForward(2,false);
                output.setRelayReverse(2,true);
            }
            if (!bJ12Button06 && !bJ12Button07)
            {
                output.setRelayForward(2,false);
                output.setRelayReverse(2,false);
            }
            debug.writeToScreen("Shooter Angle :" + shooterAngle);
            debug.writeToScreen("Ramp Rod Up :" + (input.getRamp1()));
            debug.writeToScreen("Ramp Rod Down:" + (input.getRamp2()));
			debug.writeToScreen("Angle Volt: " + Double.toString(input.getAngleVoltage() ) );
            //output.setTop(-0.9);
    }
}
